/*
  Copyright (c), 2001-2022, Shenshu Tech. Co., Ltd.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/ioctl.h>

#include "sample_comm.h"
#include "securec.h"

static volatile sig_atomic_t g_sig_flag = 0;

#define X_ALIGN 16
#define Y_ALIGN 2


#define VB_RAW_CNT_NONE     0
#define VB_LINEAR_RAW_CNT   5
#define VB_WDR_RAW_CNT      8
#define VB_MULTI_RAW_CNT    15
#define VB_YUV_ROUTE_CNT    10
#define VB_DOUBLE_YUV_CNT   15
#define VB_MULTI_YUV_CNT    30

static sample_vo_cfg g_vo_cfg = {
    .vo_dev = SAMPLE_VO_DEV_UHD,
    .vo_intf_type = OT_VO_INTF_HDMI,
    .intf_sync = OT_VO_OUT_1080P60,
    .bg_color = COLOR_RGB_BLACK,
    .pix_format = OT_PIXEL_FORMAT_YVU_SEMIPLANAR_420,
    .disp_rect = {0, 0, 1920, 1080},
    .image_size = {1920, 1080},
    .vo_part_mode = OT_VO_PARTITION_MODE_SINGLE,
    .dis_buf_len = 3, /* 3: def buf len for single */
    .dst_dynamic_range = OT_DYNAMIC_RANGE_SDR8,
    .vo_mode = VO_MODE_1MUX,
    .compress_mode = OT_COMPRESS_MODE_NONE,
};

static sample_comm_venc_chn_param g_venc_chn_param = {
    .frame_rate = 30, /* 30 is a number */
    .stats_time = 1,  /* 1 is a number */
    .gop = 30, /* 30 is a number */
    .venc_size = {1920, 1080},
    .size = PIC_1080P,
    .profile = 0,
    .is_rcn_ref_share_buf = TD_FALSE,
    .gop_attr = {
        .gop_mode = OT_VENC_GOP_MODE_NORMAL_P,
        .normal_p = {2},
    },
    .type = OT_PT_H264,
    .rc_mode = SAMPLE_RC_VBR,
};

#include "sample_common_ive.h"

#define OT_SAMPLE_IVE_KCF_ROI_NUM                   64
#define OT_SAMPLE_IVE_KCF_FRAME_NUM                 2
#define OT_SAMPLE_IVE_KCF_NODE_MAX_NUM              64
#define OT_SAMPLE_IVE_KCF_GAUSS_PEAK_TOTAL_SIZE     455680
#define OT_SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE     832
#define OT_SAMPLE_IVE_KCF_COS_WINDOW_SIZE           64
#define OT_SAMPLE_IVE_KCF_TEMP_BUF_SIZE             47616
#define OT_SAMPLE_IVE_KCF_ROI_PADDING_MAX_WIDTH     1024
#define OT_SAMPLE_IVE_KCF_ROI_PADDING_MIN_WIDTH     40
#define OT_SAMPLE_IVE_KCF_ROI_PADDING_MAX_HEIGHT    1024
#define OT_SAMPLE_IVE_KCF_ROI_PADDING_MIN_HEIGHT    40
#define OT_SAMPLE_IVE_KCF_QUEUE_LEN                     16
#define OT_SAMPLE_IVE_KCF_NUM_THIRTY_TWO            32
#define OT_SAMPLE_IVE_KCF_INTERP_FACTOR             (0.02 * 1024 * 32)
#define OT_SAMPLE_IVE_KCF_LAMDA                     10
#define OT_SAMPLE_IVE_KCF_SIGMA                     (0.5 * 256)
#define OT_SAMPLE_IVE_KCF_NORM_TRUNC_ALFA           (0.2 * 4096)
#define OT_SAMPLE_IVE_KCF_RESP_THR                  32
#define OT_SAMPLE_IVE_KCF_PADDING                   (1.5 * 32)
#define OT_SAMPLE_IVE_KCF_QUERY_SLEEP               100
#define OT_SAMPLE_IVE_KCF_MILLIC_SEC                20000
#define OT_SAMPLE_IVE_KCF_TRACK_CAR_INDEX           7
#define OT_SAMPLE_IVE_KCF_NMS_THRESHOLD             0.3
#define OT_SAMPLE_IVE_KCF_STOP_SIGNAL               2
#define OT_SAMPLE_IVE_KCF_NO_STOP_SIGNAL            3

#define OT_SAMPLE_IVE_KCF_SVP_NPU_MAX_ROI_NUM_OF_CLASS     50
#define OT_SAMPLE_IVE_KCF_SVP_NPU_MAX_CLASS_NUM            100

typedef struct {
    ot_ive_roi_info roi_info[OT_SAMPLE_IVE_KCF_ROI_NUM];
    td_u32 roi_num;
    ot_svp_mem_info total_mem;
    ot_svp_mem_info list_mem;
    ot_svp_mem_info cos_win_x;
    ot_svp_mem_info cos_win_y;
    ot_svp_mem_info gauss_peak;
    ot_ive_kcf_obj_list obj_list;
    ot_ive_kcf_proc_ctrl kcf_proc_ctrl;
    ot_ive_kcf_bbox bbox[OT_SAMPLE_IVE_KCF_ROI_NUM];
    td_u32 bbox_obj_num;
    ot_ive_kcf_bbox_ctrl kcf_bbox_ctrl;
    td_u3q5 padding;
    td_u8 reserved;

    td_bool is_new_det;
    td_bool is_first_det;
    td_bool needTrain;
    td_bool is_train;
    float Kcf_x;
    float Kcf_y;
    float Kcf_w;
    float Kcf_h;
} st_ot_sample_ive_kcf_info;
st_ot_sample_ive_kcf_info kcf_info;

// float Kcf_x = 0.0f, Kcf_y = 0.0f, Kcf_w = 0.0f, Kcf_h = 0.0f;

static td_void sample_get_char(td_void)
{
    if (g_sig_flag == 1) {
        return;
    }
    sample_pause();
}

int vgsdraw(ot_video_frame_info* pframe, int x, int y, int w, int h) {

    td_s32 ret;
    ot_vgs_handle h_handle = -1;
    ot_vgs_task_attr vgs_task_attr = { 0 };
    static ot_vgs_line stLines[4];
    int thick = 2;

    int color = 0x00FF00;

    int linecount = 0;
    int xs = x & 0xFFFE;
    int ys = y & 0xFFFE;
    int xe = (xs + w) & 0xFFFE;
    int ye = (ys + h) & 0xFFFE;

    // 上方的线条
    stLines[linecount].color = color;
    stLines[linecount].thick = thick;
    stLines[linecount].start_point.x = xs;
    stLines[linecount].start_point.y = ys;
    stLines[linecount].end_point.x = xe;
    stLines[linecount].end_point.y = ys;
    linecount++;

    // 左边的线条
    stLines[linecount].color = color;
    stLines[linecount].thick = thick;
    stLines[linecount].start_point.x = xs;
    stLines[linecount].start_point.y = ys;
    stLines[linecount].end_point.x = xs;
    stLines[linecount].end_point.y = ye;
    linecount++;

    // 右边线条
    stLines[linecount].color = color;
    stLines[linecount].thick = thick;
    stLines[linecount].start_point.x = xe;
    stLines[linecount].start_point.y = ys;
    stLines[linecount].end_point.x = xe;
    stLines[linecount].end_point.y = ye;
    linecount++;

    // 下边线条
    stLines[linecount].color = color;
    stLines[linecount].thick = thick;
    stLines[linecount].start_point.x = xs;
    stLines[linecount].start_point.y = ye;
    stLines[linecount].end_point.x = xe;
    stLines[linecount].end_point.y = ye;
    linecount++;


    ret = ss_mpi_vgs_begin_job(&h_handle);
    if (ret != TD_SUCCESS) {
        return TD_FAILURE;
    }
    if (memcpy_s(&vgs_task_attr.img_in, sizeof(ot_video_frame_info), pframe,
        sizeof(ot_video_frame_info)) != EOK) {
        return TD_FAILURE;
    }
    if (memcpy_s(&vgs_task_attr.img_out, sizeof(ot_video_frame_info), pframe,
        sizeof(ot_video_frame_info)) != EOK) {
        return TD_FAILURE;
    }

    ret = ss_mpi_vgs_add_draw_line_task(h_handle, &vgs_task_attr, stLines,
        linecount);
    if (ret != TD_SUCCESS) {
        return ret;
    }

    if (ret != TD_SUCCESS) {
        ss_mpi_vgs_cancel_job(h_handle);
        return TD_FAILURE;
    }

    /* step5: start VGS work */
    ret = ss_mpi_vgs_end_job(h_handle);
    if (ret != TD_SUCCESS) {
        ss_mpi_vgs_cancel_job(h_handle);
        return TD_FAILURE;
    }

    return ret;
}

static td_void sys_vb_config(ot_size* size, ot_vb_cfg* vb_cfg, ot_vi_video_mode video_mode,
    td_u32 yuv_cnt, td_u32 raw_cnt)
{
    ot_vb_calc_cfg calc_cfg;
    ot_pic_buf_attr buf_attr;

    (td_void)memset_s(vb_cfg, sizeof(ot_vb_cfg), 0, sizeof(ot_vb_cfg));
    vb_cfg->max_pool_cnt = 128; /* 128 blks */

    /* default YUV pool: SP420 + compress_seg */
    buf_attr.width = size->width;
    buf_attr.height = size->height;
    buf_attr.align = OT_DEFAULT_ALIGN;
    buf_attr.bit_width = OT_DATA_BIT_WIDTH_8;
    buf_attr.pixel_format = OT_PIXEL_FORMAT_YVU_SEMIPLANAR_420;
    buf_attr.compress_mode = OT_COMPRESS_MODE_SEG;
    ot_common_get_pic_buf_cfg(&buf_attr, &calc_cfg);

    vb_cfg->common_pool[0].blk_size = calc_cfg.vb_size;
    vb_cfg->common_pool[0].blk_cnt = yuv_cnt;

    /* default raw pool: raw12bpp + compress_line */
    buf_attr.pixel_format = OT_PIXEL_FORMAT_RGB_BAYER_12BPP;
    buf_attr.compress_mode = (video_mode == OT_VI_VIDEO_MODE_NORM ? OT_COMPRESS_MODE_LINE : OT_COMPRESS_MODE_NONE);
    ot_common_get_pic_buf_cfg(&buf_attr, &calc_cfg);
    vb_cfg->common_pool[1].blk_size = calc_cfg.vb_size;
    vb_cfg->common_pool[1].blk_cnt = raw_cnt;
}

static td_s32 sample_sys_init(ot_vi_vpss_mode_type mode_type, ot_vi_video_mode video_mode,
    td_u32 yuv_cnt, td_u32 raw_cnt)
{
    td_s32 ret;
    ot_size size;
    ot_vb_cfg vb_cfg;
    td_u32 supplement_config;
    sample_sns_type sns_type = SENSOR0_TYPE;

    sample_comm_vi_get_size_by_sns_type(sns_type, &size);
    sys_vb_config(&size, &vb_cfg, video_mode, yuv_cnt, raw_cnt);

    supplement_config = OT_VB_SUPPLEMENT_BNR_MOT_MASK;
    ret = sample_comm_sys_init_with_vb_supplement(&vb_cfg, supplement_config);
    if (ret != TD_SUCCESS) {
        return TD_FAILURE;
    }

    ret = sample_comm_vi_set_vi_vpss_mode(mode_type, video_mode);
    if (ret != TD_SUCCESS) {
        return TD_FAILURE;
    }
    return TD_SUCCESS;
}

static td_s32 sstart_vpss(ot_vpss_grp grp, ot_size* in_size)
{
    td_s32 ret;
    ot_low_delay_info low_delay_info;
    ot_vpss_grp_attr grp_attr;
    ot_vpss_chn_attr chn_attr;
    td_bool chn_enable[OT_VPSS_MAX_PHYS_CHN_NUM] = { TD_TRUE, TD_FALSE, TD_FALSE, TD_FALSE };

    sample_comm_vpss_get_default_grp_attr(&grp_attr);
    grp_attr.max_width = in_size->width;
    grp_attr.max_height = in_size->height;
    sample_comm_vpss_get_default_chn_attr(&chn_attr);
    chn_attr.width = 1920;
    chn_attr.height = 1080;
    chn_attr.compress_mode = OT_COMPRESS_MODE_NONE;
    chn_attr.depth = 1;
    ot_vpss_chn_attr chn_attrex[2];

    ret = sample_common_vpss_start(grp, chn_enable, &grp_attr, &chn_attr, OT_VPSS_MAX_PHYS_CHN_NUM);
    if (ret != TD_SUCCESS) {
        return ret;
    }

    return TD_SUCCESS;
}

static td_void sample_ive_kcf_info_common_init(st_ot_sample_ive_kcf_info* kcf_info, td_u32 size)
{
    kcf_info->list_mem.phys_addr = kcf_info->total_mem.phys_addr;
    kcf_info->list_mem.virt_addr = kcf_info->total_mem.virt_addr;
    kcf_info->list_mem.size = size;
    kcf_info->gauss_peak.phys_addr = kcf_info->list_mem.phys_addr + size;
    kcf_info->gauss_peak.virt_addr = kcf_info->list_mem.virt_addr + size;
    kcf_info->gauss_peak.size = OT_SAMPLE_IVE_KCF_GAUSS_PEAK_TOTAL_SIZE;
    kcf_info->cos_win_x.phys_addr = kcf_info->gauss_peak.phys_addr + OT_SAMPLE_IVE_KCF_GAUSS_PEAK_TOTAL_SIZE;
    kcf_info->cos_win_x.virt_addr = kcf_info->gauss_peak.virt_addr + OT_SAMPLE_IVE_KCF_GAUSS_PEAK_TOTAL_SIZE;
    kcf_info->cos_win_x.size = OT_SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;
    kcf_info->cos_win_y.phys_addr = kcf_info->cos_win_x.phys_addr + OT_SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;
    kcf_info->cos_win_y.virt_addr = kcf_info->cos_win_x.virt_addr + OT_SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;
    kcf_info->cos_win_y.size = OT_SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;
    kcf_info->kcf_proc_ctrl.tmp_buf.phys_addr = kcf_info->cos_win_y.phys_addr +
        OT_SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;
    kcf_info->kcf_proc_ctrl.tmp_buf.virt_addr = kcf_info->cos_win_y.virt_addr +
        OT_SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;
    kcf_info->kcf_proc_ctrl.tmp_buf.size = OT_SAMPLE_IVE_KCF_TEMP_BUF_SIZE;
    kcf_info->kcf_proc_ctrl.csc_mode = OT_IVE_CSC_MODE_VIDEO_BT709_YUV_TO_RGB;
    kcf_info->kcf_proc_ctrl.interp_factor = OT_SAMPLE_IVE_KCF_INTERP_FACTOR;
    kcf_info->kcf_proc_ctrl.lamda = OT_SAMPLE_IVE_KCF_LAMDA;
    kcf_info->kcf_proc_ctrl.sigma = OT_SAMPLE_IVE_KCF_SIGMA;
    kcf_info->kcf_proc_ctrl.norm_trunc_alfa = OT_SAMPLE_IVE_KCF_NORM_TRUNC_ALFA;
    kcf_info->kcf_proc_ctrl.response_threshold = OT_SAMPLE_IVE_KCF_RESP_THR;
    kcf_info->padding = OT_SAMPLE_IVE_KCF_PADDING;
    kcf_info->kcf_bbox_ctrl.max_bbox_num = OT_SAMPLE_IVE_KCF_NODE_MAX_NUM;
    kcf_info->kcf_bbox_ctrl.response_threshold = 0;
}

void init_kcf()
{
    td_s32 ret = OT_ERR_IVE_NULL_PTR;
    (td_void)memset_s(&kcf_info, sizeof(st_ot_sample_ive_kcf_info), 0, sizeof(st_ot_sample_ive_kcf_info));
    td_u32 total_size;
    td_u32 size;
    ret = ss_mpi_ive_kcf_get_mem_size(OT_SAMPLE_IVE_KCF_NODE_MAX_NUM, &size);

    /* (HOGFeatrue + Alpha + DstBuf) + Guasspeak + CosWinX + CosWinY + TmpBuf */
    total_size = size + OT_SAMPLE_IVE_KCF_GAUSS_PEAK_TOTAL_SIZE + OT_SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE *
        OT_SAMPLE_NUM_TWO + OT_SAMPLE_IVE_KCF_TEMP_BUF_SIZE;
    ret = sample_common_ive_create_mem_info(&kcf_info.total_mem, total_size);
    (td_void)memset_s((td_u8*)(td_uintptr_t)kcf_info.total_mem.virt_addr, total_size, 0x0, total_size);
    sample_ive_kcf_info_common_init(&kcf_info, size);
    ret = ss_mpi_ive_kcf_create_obj_list(&kcf_info.list_mem, OT_SAMPLE_IVE_KCF_NODE_MAX_NUM, &kcf_info.obj_list);
    ret = ss_mpi_ive_kcf_create_gauss_peak(kcf_info.padding, &kcf_info.gauss_peak);
    ret = ss_mpi_ive_kcf_create_cos_win(&kcf_info.cos_win_x, &kcf_info.cos_win_y);
    kcf_info.is_first_det = TD_FALSE;
    kcf_info.needTrain = TD_FALSE;
}
static td_s32 sample_ive_kcf_query_task(ot_ive_handle handle)
{
    td_bool is_finish = TD_FALSE;
    td_bool is_block = TD_TRUE;
    td_s32 ret;
    ret = ss_mpi_ive_query(handle, &is_finish, is_block);
    while (ret == OT_ERR_IVE_QUERY_TIMEOUT) {
        usleep(OT_SAMPLE_IVE_KCF_QUERY_SLEEP);
        ret = ss_mpi_ive_query(handle, &is_finish, is_block);
    }
    sample_svp_check_exps_return(ret != TD_SUCCESS, ret, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error(%#x),ss_mpi_ive_query failed!\n", ret);
    return TD_SUCCESS;
}

typedef struct {
    ot_ive_roi_info roi[OT_SAMPLE_IVE_KCF_ROI_NUM];
    td_u32 roi_num;
} ot_sample_ive_kcf_roi_info;

typedef struct {
    ot_ive_kcf_bbox bbox[OT_SAMPLE_IVE_KCF_ROI_NUM];
    td_u32 bbox_num;
} ot_sample_ive_kcf_bbox_info;
static td_float sample_ive_kcf_calc_iou(ot_svp_rect_s24q8* rect1, ot_svp_rect_s24q8* rect2)
{
    td_s32 min_x, min_y, max_x, max_y;
    td_float area1, area2, inter_area, iou;
    td_s32 width, height;

    min_x = ot_sample_ive_max(rect1->x / OT_SAMPLE_QUARTER_OF_1M, rect2->x / OT_SAMPLE_QUARTER_OF_1M);
    min_y = ot_sample_ive_max(rect1->y / OT_SAMPLE_QUARTER_OF_1M, rect2->y / OT_SAMPLE_QUARTER_OF_1M);
    max_x = ot_sample_ive_min(rect1->x / OT_SAMPLE_QUARTER_OF_1M + rect1->width,
        rect2->x / OT_SAMPLE_QUARTER_OF_1M + rect2->width);
    max_y = ot_sample_ive_min(rect1->y / OT_SAMPLE_QUARTER_OF_1M + rect1->height,
        rect2->y / OT_SAMPLE_QUARTER_OF_1M + rect2->height);

    width = max_x - min_x + 1;
    height = max_y - min_y + 1;
    width = (width > 0) ? width : 0;
    height = (height > 0) ? height : 0;
    inter_area = (td_float)((td_u32)width * (td_u32)height);
    area1 = (td_float)(rect1->width * rect1->height);
    area2 = (td_float)(rect2->width * rect2->height);
    iou = inter_area / (area1 + area2 - inter_area);

    return iou;
}
static td_s32 sample_ive_kcf_updata_bbox_info(td_bool is_suppressed, ot_ive_roi_info* roi,
    ot_ive_kcf_bbox* bbox)
{
    td_s32 ret;
    td_bool is_track_ok = TD_FALSE;
    if (is_suppressed != 0) {
        ret = ss_mpi_ive_kcf_judge_obj_bbox_track_state(roi, bbox, &is_track_ok);
        sample_svp_check_exps_return(ret != TD_SUCCESS, ret, SAMPLE_SVP_ERR_LEVEL_ERROR,
            "Error(%#x),ss_mpi_ive_kcf_judge_obj_bbox_track_state failed!\n", ret);
        if (is_track_ok == TD_TRUE) {
            bbox->roi_info.roi.x = roi->roi.x;
            bbox->roi_info.roi.y = roi->roi.y;
            bbox->roi_info.roi.width = roi->roi.width;
            bbox->roi_info.roi.height = roi->roi.height;
            bbox->is_roi_refresh = TD_TRUE;

            roi->roi_id = 0;
        }
        else {
            bbox->is_track_ok = TD_FALSE;
        }
    }
    return TD_SUCCESS;
}
static td_s32 sample_ive_kcf_obj_iou(ot_sample_ive_kcf_roi_info* roi_info,
    ot_sample_ive_kcf_bbox_info* bbox_info, td_u32 max_roi_num, ot_sample_ive_kcf_roi_info* new_roi_info)
{
    td_s32 ret = TD_SUCCESS;
    td_u32 num = 0;
    td_u32 i, j;

    if (bbox_info->bbox_num == 0) {
        ret = memcpy_s(new_roi_info, sizeof(ot_sample_ive_kcf_roi_info), roi_info, sizeof(ot_sample_ive_kcf_roi_info));
        sample_svp_check_exps_return(ret != EOK, OT_ERR_IVE_ILLEGAL_PARAM, SAMPLE_SVP_ERR_LEVEL_ERROR,
            "memcpy_s roi_info failed!\n");
        return ret;
    }

    if (roi_info->roi_num == 0) {
        new_roi_info->roi_num = 0;
        return ret;
    }

    for (i = 0; (i < roi_info->roi_num) && (i < OT_SAMPLE_IVE_KCF_ROI_NUM); i++) {
        for (j = 0; (j < bbox_info->bbox_num) && (j < OT_SAMPLE_IVE_KCF_ROI_NUM); j++) {
            td_bool is_suppressed;
            td_float iou_val;

            if (roi_info->roi[i].roi_id == 0) {
                break;
            }
            if ((bbox_info->bbox[j].roi_info.roi_id == 0) || (bbox_info->bbox[j].is_track_ok == TD_FALSE)) {
                continue;
            }

            iou_val = sample_ive_kcf_calc_iou(&roi_info->roi[i].roi, &bbox_info->bbox[j].roi_info.roi);
            is_suppressed = (td_bool)(iou_val >= OT_SAMPLE_IVE_KCF_NMS_THRESHOLD);
            ret = sample_ive_kcf_updata_bbox_info(is_suppressed, &roi_info->roi[i], &bbox_info->bbox[j]);
            sample_svp_check_exps_return(ret != TD_SUCCESS, ret, SAMPLE_SVP_ERR_LEVEL_ERROR,
                "Error(%#x), sample_ive_kcf_updata_bbox_info failed!\n", ret);
            if (is_suppressed == TD_TRUE) {
                break;
            }
        }

        if (num == max_roi_num) {
            sample_svp_trace_debug("reaching the maxRoiNum(%d), the rest roi will be cast away!\n", max_roi_num);
            new_roi_info->roi_num = num;
            return TD_SUCCESS;
        }
        /* u32RoiId!=0, means it hasn't been suppressed */
        if (roi_info->roi[i].roi_id != 0) {
            ret = memcpy_s(&new_roi_info->roi[num++], sizeof(ot_ive_roi_info),
                &roi_info->roi[i], sizeof(ot_ive_roi_info));
            sample_svp_check_exps_return(ret != EOK, OT_ERR_IVE_ILLEGAL_PARAM, SAMPLE_SVP_ERR_LEVEL_ERROR,
                "memcpy_s roi_info failed!\n");
        }
    }
    new_roi_info->roi_num = num;
    return ret;
}
static td_void sample_ive_rect_to_point(ot_ive_kcf_bbox bbox[], td_u32 bbox_obj_num,
    ot_sample_svp_rect_info* rect)
{
    td_u32 i;
    td_u32 tmp_ux, tmp_uy;
    td_s32 tmp_sx, tmp_sy;

    rect->num = 0;
    for (i = 0; (i < bbox_obj_num) && (i < OT_SVP_RECT_NUM); i++) {
        tmp_sx = bbox[i].roi_info.roi.x / OT_SAMPLE_QUARTER_OF_1M;
        tmp_sy = bbox[i].roi_info.roi.y / OT_SAMPLE_QUARTER_OF_1M;

        if (tmp_sx < 0) {
            tmp_ux = *((td_u32*)&tmp_sx);
            tmp_ux = tmp_ux & (~1);
            tmp_sx = *((td_s32*)&tmp_ux);
        }
        if (tmp_sy < 0) {
            tmp_uy = *((td_u32*)&tmp_sy);
            tmp_uy = tmp_uy & (~1);
            tmp_sy = *((td_s32*)&tmp_sy);
        }
        if (tmp_sx > 0) {
            tmp_ux = (td_u32)tmp_sx;
            tmp_ux = tmp_ux & (~1);
            tmp_sx = tmp_ux;
        }
        if (tmp_sy > 0) {
            tmp_uy = (td_u32)tmp_sy;
            tmp_uy = tmp_uy & (~1);
            tmp_sy = tmp_uy;
        }

        rect->rect[i].point[0].x = tmp_sx;
        rect->rect[i].point[0].y = tmp_sy;
        if ((rect->rect[i].point[0].x < 0) || (rect->rect[i].point[0].y < 0)) {
            rect->rect[i].point[0].x = 0;
            rect->rect[i].point[0].y = 0;
        }
        rect->rect[i].point[1].x = rect->rect[i].point[0].x + (bbox[i].roi_info.roi.width & (~1));
        rect->rect[i].point[1].y = rect->rect[i].point[0].y;
        rect->rect[i].point[OT_SAMPLE_IDX_TWO].x = rect->rect[i].point[0].x +
            (bbox[i].roi_info.roi.width & (~1));
        rect->rect[i].point[OT_SAMPLE_IDX_TWO].y = rect->rect[i].point[0].y +
            (bbox[i].roi_info.roi.height & (~1));
        rect->rect[i].point[OT_SAMPLE_IDX_THREE].x = rect->rect[i].point[0].x;
        rect->rect[i].point[OT_SAMPLE_IDX_THREE].y = rect->rect[i].point[0].y +
            (bbox[i].roi_info.roi.height & (~1));

        rect->num++;
    }
}

pthread_t kcf_pid;
pthread_t WebStream_pid;
int running = 0;
void* kcf_proc(void* parg)
{
    td_s32 milli_sec = 40;
    td_s32  ret;
    ot_vpss_grp grp = 0;
    ot_vpss_chn chn = 0;
    ot_video_frame_info frame_info;
    kcf_info.is_train = TD_FALSE;
    kcf_info.needTrain = TD_FALSE;
    ot_ive_handle handle;
    ot_ive_kcf_bbox bbox;
    static ot_sample_ive_kcf_bbox_info bbox_info = { 0 };
    static ot_sample_ive_kcf_roi_info roi_info = { 0 };
    static ot_sample_ive_kcf_roi_info new_roi_info = { 0 };
    init_kcf();
    while (running)
    {
        ot_usleep(1000);
        ret = ss_mpi_vpss_get_chn_frame(grp, chn, &frame_info, milli_sec);
        if (ret != TD_SUCCESS) {
            continue;
        }
        int w = frame_info.video_frame.width;
        int h = frame_info.video_frame.height;

        ot_svp_src_img src;
        src.width = w;
        src.height = h;
        src.phys_addr[0] = frame_info.video_frame.phys_addr[0];
        src.phys_addr[1] = frame_info.video_frame.phys_addr[1];
        src.phys_addr[2] = frame_info.video_frame.phys_addr[2];
        src.stride[0] = frame_info.video_frame.stride[0];
        src.stride[1] = frame_info.video_frame.stride[1];
        src.stride[2] = frame_info.video_frame.stride[2];
        src.type = OT_SVP_IMG_TYPE_YUV420SP;

        int framelen = w * h * 3 / 2;
        if (kcf_info.is_train)
        {

            if (kcf_info.obj_list.track_obj_num != 0 || kcf_info.obj_list.train_obj_num != 0)
            {
                memset(&bbox_info, 0, sizeof(ot_sample_ive_kcf_bbox_info));
                ret = ss_mpi_ive_kcf_proc(&handle, &src, &kcf_info.obj_list, &kcf_info.kcf_proc_ctrl, TD_TRUE);
                ret = sample_ive_kcf_query_task(handle);
                ret = ss_mpi_ive_kcf_get_obj_bbox(&kcf_info.obj_list, bbox_info.bbox, &bbox_info.bbox_num, &kcf_info.kcf_bbox_ctrl);
                printf("bbox_info.bbox_num:%d \n", bbox_info.bbox_num);
                ot_sample_svp_rect_info rect = { 0 };
                sample_ive_rect_to_point(bbox_info.bbox, bbox_info.bbox_num, &rect);
                if (rect.num)
                {
                    int x = rect.rect[0].point[0].x;
                    int y = rect.rect[0].point[0].y;
                    int w = rect.rect[0].point[1].x - rect.rect[0].point[0].x;
                    int h = rect.rect[0].point[2].y - rect.rect[0].point[0].y;
                    // printf(" %d %d %d %d\n", x, y, w, h);
                    vgsdraw(&frame_info, x, y, w, h);
                }
            }
        }

        if (kcf_info.needTrain)
        {
            memset(&bbox_info, 0, sizeof(ot_sample_ive_kcf_bbox_info));
            kcf_info.needTrain = TD_FALSE;

            roi_info.roi[0].roi.x = kcf_info.Kcf_x * w * OT_SAMPLE_QUARTER_OF_1M;
            roi_info.roi[0].roi.y = kcf_info.Kcf_y * h * OT_SAMPLE_QUARTER_OF_1M;
            roi_info.roi[0].roi.width = kcf_info.Kcf_w * w;
            roi_info.roi[0].roi.height = kcf_info.Kcf_h * h;
            roi_info.roi_num = 1;

            new_roi_info.roi[0].roi.x = kcf_info.Kcf_x * OT_SAMPLE_QUARTER_OF_1M;
            new_roi_info.roi[0].roi.y = kcf_info.Kcf_y * h * OT_SAMPLE_QUARTER_OF_1M;
            new_roi_info.roi[0].roi.width = kcf_info.Kcf_w * w;
            new_roi_info.roi[0].roi.height = kcf_info.Kcf_h * h;
            new_roi_info.roi_num = 1;

            sample_ive_kcf_obj_iou(&roi_info, &bbox_info, OT_SAMPLE_IVE_KCF_ROI_NUM, &new_roi_info);
            ret = ss_mpi_ive_kcf_obj_update(&kcf_info.obj_list, bbox_info.bbox, bbox_info.bbox_num);

            ret = ss_mpi_ive_kcf_get_train_obj(kcf_info.padding, &new_roi_info.roi, new_roi_info.roi_num, &kcf_info.cos_win_x, &kcf_info.cos_win_y, &kcf_info.gauss_peak, &kcf_info.obj_list);
            printf("ss_mpi_ive_kcf_get_train_obj %s %d %08x \n", __FUNCTION__, __LINE__, ret);
            kcf_info.is_train = TD_TRUE;
        }

        ret = ss_mpi_vo_send_frame(0, 0, &frame_info, milli_sec);
        ret = ss_mpi_venc_send_frame(0, &frame_info, milli_sec);
        ss_mpi_vpss_release_chn_frame(grp, chn, &frame_info);
    }
    return NULL;
}

void* WenStream_proc(void* parg)
{
    td_s32 ret;
    ot_venc_stream stream;
    ot_venc_chn_status stat;
    ot_venc_chn chn = 0;
    while (running)
    {
        if (memset_s(&stream, sizeof(stream), 0, sizeof(stream)) != EOK) {
            printf("call memset_s error\n");
        }
        ret = ss_mpi_venc_query_status(chn, &stat);
        if (ret != TD_SUCCESS) {
            continue;
        }

        if (stat.cur_packs == 0) {
            continue;
        }

        stream.pack = (ot_venc_pack*)malloc(sizeof(ot_venc_pack) * stat.cur_packs);
        if (stream.pack == TD_NULL) {
            printf("malloc stream pack failed!\n");
            continue;
        }
        stream.pack_cnt = stat.cur_packs;
        ret = ss_mpi_venc_get_stream(chn, &stream, 20);
        if (ret != TD_SUCCESS) {
            free(stream.pack);
            stream.pack = TD_NULL;
            printf("ss_mpi_venc_get_stream failed with %#x!\n", ret);
            continue;
        }
        for (int i = 0; i < stream.pack_cnt; i++) {
            WebStream_sendStream(
                stream.pack[i].addr + stream.pack[i].offset,
                stream.pack[i].len - stream.pack[i].offset, stream.pack[i].pts);
        }
        ret = ss_mpi_venc_release_stream(chn, &stream);
        if (ret != TD_SUCCESS) {
            printf("ss_mpi_venc_release_stream failed!\n");
            free(stream.pack);
            stream.pack = TD_NULL;
            continue;
        }
        free(stream.pack);
        stream.pack = TD_NULL;
        ot_usleep(100);
    }
    return NULL;
}

//输入参数为百分比
void WebCbPos(float x, float y, float w, float h)
{
    printf("%f %f %f %f\n", x, y, w, h);
    if (w > 0.01f && h > 0.01f)
    {
        kcf_info.Kcf_x = x;
        kcf_info.Kcf_y = y;
        kcf_info.Kcf_w = w;
        kcf_info.Kcf_h = h;
        kcf_info.needTrain = TD_TRUE;
    }
}
td_s32 main(td_s32 argc, td_char* argv[])
{

    ss_mpi_isp_exit(0);
    td_s32 ret;
    ot_vi_vpss_mode_type mode_type = OT_VI_OFFLINE_VPSS_OFFLINE;
    ot_vi_video_mode video_mode = OT_VI_VIDEO_MODE_NORM;
    sample_sns_type sns_type = SENSOR0_TYPE;
    sample_vi_cfg vi_cfg;
    ot_size in_size;
    td_bool chn_enable[OT_VPSS_MAX_PHYS_CHN_NUM] = { TD_TRUE, TD_FALSE, TD_FALSE, TD_FALSE };
    ret = sample_sys_init(mode_type, video_mode, VB_YUV_ROUTE_CNT, VB_LINEAR_RAW_CNT);
    sample_comm_vi_get_size_by_sns_type(sns_type, &in_size);
    sample_comm_vi_get_default_vi_cfg(sns_type, &vi_cfg);
    ret = sample_comm_vi_start_vi(&vi_cfg);
    sample_comm_vi_bind_vpss(0, 0, 0, 0);
    sstart_vpss(0, &in_size);

    g_venc_chn_param.venc_size.width = 1920;
    g_venc_chn_param.venc_size.height = 1080;
    g_venc_chn_param.size = PIC_1080P;
    ret = sample_comm_venc_start(0, &g_venc_chn_param);
    sample_comm_vo_start_vo(&g_vo_cfg);
    WebStreamInit(80, "/setpos", WebCbPos);
    running = 1;
    pthread_create(&kcf_pid, 0, kcf_proc, NULL);
    pthread_create(&WebStream_pid, 0, WenStream_proc, NULL);
    sample_get_char();
    running = 0;
    pthread_join(kcf_pid, 0);
    pthread_join(WebStream_pid, 0);
    sample_comm_vo_stop_vo(&g_vo_cfg);
    sample_comm_vi_un_bind_vpss(0, 0, 0, 0);
    sample_common_vpss_stop(0, chn_enable, OT_VPSS_MAX_PHYS_CHN_NUM);
    sample_comm_vi_stop_vi(&vi_cfg);
    sample_comm_sys_exit();
    return 0;
}
